#ifndef MARKERLOCATOR_H
#define MARKERLOCATOR_H

#include <vector>
#include <opencv2/opencv.hpp>

class MarkerLocator
{
    public:
        /** Default constructor */
        MarkerLocator(cv::Mat r_cameraMatrix, cv::Mat r_distCoeff, cv::Mat r_robotLoc, float r_markerSize);
        /** Default destructor */
        ~MarkerLocator();

        void UpdateMarkers(cv::Mat r_frame); //!< Updates the list of markers based on which ones are visible
        cv::Mat GetLocation(int r_markerId); //!< Gets the range and bearing for given marker

        #ifdef GUI
        void SetDrawMats(cv::Mat r_binary, cv::Mat r_markers); //!< Sets the mats the the marker locator will draw on.
        #endif
    private:
        struct Marker
        {
            int m_id; //!< Printed ID of the marker
            cv::Mat m_location; //!< Holds the range and bearing
        };

	#ifdef GUI
        cv::Mat m_binaryOut; //!< Visual display of the marker locator
        cv::Mat m_markersOut; //!< Display of the visible markers
	#endif

        bool GetBit(cv::Mat r_marker, int r_x, int r_y, int r_width);
        int GetMarkerId(cv::Mat r_marker);
        void ChangeSpace(cv::Mat& r_point);
        void TVectToRangeBaring(cv::Mat r_tVec, double& rr_range, double& rr_bearing);

        cv::Mat m_cameraMatrix; //!< The camera matrix for the camera that will be used.
        cv::Mat m_distCoeff; //!< Distortion coefficients for the camera that is being used.
        cv::Mat m_robotForward; //!< Forward vector of the robot.
        cv::Mat m_robotSide; //!< Side vector of the robot.
        cv::Mat m_robotLoc; //!< Location of the robot relative the camera.
        float m_markerSize; //!< Physical size of a marker edge in desired units.

        std::vector<Marker> m_knownMarkers; //!< Used to access markers based on their ID.

        // These are here to avoid needless reallocation
        cv::Mat m_markerWorldPoints; //!< Contains the 3d points of a given marker. Used for solvePnP.
        std::vector<cv::Point2f> m_markerImagePoints; //!< Contains the 2d points of a given marker. Used for perspective warping.
        cv::Mat m_perspectiveMap;
};

#endif // MARKERLOCATOR_H
